#include "BVHMotion.h"

BVHMotion::BVHMotion(){
	mFrameCount = 0;
	mFrameRate = 0.0f;
	mFrameTime =0.0f;
	mDataCount = 0;
	//mRoot = NULL;
	mOrder = new vector<Node*>();
}


void BVHMotion::LoadFromFile(string input){
	
	if(input.compare("") != 0)
	{
		ifstream fp_in;  // declarations of streams fp_in and fp_out
		fp_in.open(input.c_str(), ios::in);    // open the streams
		float mass = 0.0f;
		
			vector<string> tokens;
			string test;
			getline(fp_in, test);
			Tokenize(test, tokens);
			if(tokens[0].compare("HIERARCHY") == 0)
			{
					mRoot = CreateJoint(&fp_in);
			}

			//getline(fp_in, test);
			getline(fp_in, test);
			Tokenize(test, tokens);
			if(tokens[0].compare("MOTION") == 0)
			{
				getline(fp_in, test);
				Tokenize(test, tokens,"\t");	
				
				mFrameCount = atoi(tokens[1].c_str());

				getline(fp_in, test);
				Tokenize(test, tokens,"\t");	

				mFrameRate = atof(tokens[1].c_str());

				for(int f =0; f < mFrameCount; f++){
					
				getline(fp_in, test);
				Tokenize(test, tokens,"\t");	
					int index = 0; //The data index				
					for(int i = 0; i <  mOrder->size(); i++){
						vector<float>* frame = new vector<float>();
						for(int j =0; j < mOrder->at(i)->GetNumOfChannels();j++){
							frame->push_back((float)(atof(tokens[index++].c_str())));
						}
						mOrder->at(i)->GetFrames()->push_back(frame);
					}
				}
			}

		fp_in.close();
	}
}

Node* BVHMotion::CreateJoint(ifstream* fp_in){

	string test;
	Node* ret = NULL;
	vector<string> tokens;

	getline(*(fp_in), test);
	Tokenize(test, tokens,"\t");
	test = tokens[0];
	Tokenize(test, tokens," ");
	bool root = false;

	if(tokens[0].compare("ROOT") == 0 ||tokens[0].compare("JOINT") == 0){
		if(tokens[0].compare("ROOT") == 0 ){
			root = true;
		}
		ret = new Node();
		string* name = new string(tokens[1],0,tokens[1].length());
		ret->SetName(name);

		//Store the order
		mOrder->push_back(ret);

		//Read open parend
		getline(*(fp_in), test);

		//read in offset
		//OFFSET	0.000000	0.000000	0.000000
		getline(*(fp_in), test);
		Tokenize(test, tokens,"\t");	

		if(tokens[0].compare("OFFSET") == 0){
			ret->GetOffset()->x = atof(tokens[1].c_str());
			ret->GetOffset()->y = atof(tokens[2].c_str());
			ret->GetOffset()->z = atof(tokens[3].c_str());
		}

		//CHANNELS	6	Xposition	Yposition	Zposition	Zrotation	Xrotation	Yrotation
		getline(*(fp_in), test);
		Tokenize(test, tokens,"\t");	
		if(tokens[0].compare("CHANNELS") == 0){
			ret->SetNumOfChannels( atof(tokens[1].c_str()));
			mDataCount += ret->GetNumOfChannels();

			for(int i=0; i < ret->GetNumOfChannels();i++){
				(ret->GetChannels())[i] = ChannelStringToInt(tokens[i+2]);
				ret->GetChannelValue()[i] = 0;
			}
		}

		//Add all child joint to this node
		Node* joint = CreateJoint(fp_in);

		while(joint != NULL){
			if(ret->GetName()->compare("Hips") == 0){
				string s ="";
			}

			ret->GetJoints()->push_back(joint);
		
			
			joint = CreateJoint(fp_in);
		}

	}
	else if(tokens[0].compare("End") == 0){

		ret = new Node();
		ret->SetName(new string("End Site"));

		//Store the order
		mOrder->push_back(ret);
		getline(*(fp_in), test);
		//read in offset
		//OFFSET	0.000000	0.000000	0.000000
		getline(*(fp_in), test);
		Tokenize(test, tokens,"\t");	

		if(tokens[0].compare("OFFSET") == 0){
			ret->GetOffset()->x = atof(tokens[1].c_str());
			ret->GetOffset()->y = atof(tokens[2].c_str());
			ret->GetOffset()->z = atof(tokens[3].c_str());
		}
	
		//Read the }
		getline(*(fp_in), test);

	}
	else if(tokens[0].compare("}") == 0){

		ret = NULL;
	}

	return ret;
}


int BVHMotion::ChannelStringToInt(string channel){
	int ret = -1;
	if(channel.compare("Xposition") == 0){
		ret = Node::xTrans;
	}
	else if(channel.compare("Yposition") == 0){
		ret = Node::yTrans;
	}
	else if(channel.compare("Zposition")== 0){
		ret = Node::zTrans;
	}
	else if(channel.compare("Zrotation")== 0){
		ret = Node::zRot;
	}
	else if(channel.compare("Xrotation")== 0){
		ret = Node::xRot;
	}
	else if(channel.compare("Yrotation")== 0){
		ret = Node::yRot;
	}			

	return ret;
}


void BVHMotion::Update(float pElapsedTime){
	mFrameTime += pElapsedTime;
	if(mFrameTime >mFrameRate){
		mFrameTime -= mFrameRate;
		Node::mFrame++;
		if(Node::mFrame >= mFrameCount){
			Node::mFrame = mFrameCount-1;
		}
	}

	mRoot->Update(pElapsedTime);
}

void BVHMotion::Draw(LPDIRECT3DDEVICE9 d3ddev, LPD3DXMATRIXSTACK pStack){
	
	//::ID3DXMatrixStack::LoadIdentity();
	LPD3DXMATRIXSTACK MatrixStack;

	D3DXCreateMatrixStack(0,&MatrixStack);

	MatrixStack->LoadIdentity();
	
	mRoot->Draw(d3ddev,MatrixStack);

}